#ifndef ENCODER_SPEED_CONTROLLER_H
#define ENCODER_SPEED_CONTROLLER_H

//#define ENCODER_SPEED_DEADBAND 0 //we'll need to experiment with this depending on how much velocity varies
#define ENCODER_SPEED_INCREMENT 0
#define WHEEL_CIRCUMFERENCE (18.84*0.0254) // convert inches to m
#define ENCODER_TICKS_PER_REVOLUTION 1000.0 
//#define MIN_PARTICLE_TO_IMAGE_PERCENT 0.25		// target is too small
#define MAX_PARTICLE_TO_IMAGE_PERCENT 100.0		// target is too close

#include <math.h>
#include "WPILib.h"

class EncoderSpeedController : public SpeedController {
	// everything below can't be used by anything but this class
	// this is sorta pointless, but its main purpose is to indicate to other programmers
	//  what functions should be called externally, and what ones they shouldn't screw with
private:
	Encoder *encoder;
	SpeedController *speedController;
	
	float jaguar_speed;
	Timer timer;
	double lastTime;
	
	double a;
	double b;
	
	double measurements[5];
	int measurementNumber;
	
	double calculateEncoderSpeed() {
		// in this function we don't have to worry about time
		// because the FPGA returns pulse width for us
		
		// returns meters per second
		//cout << "Encoder period: " << encoder->GetPeriod() << "\n";
		double measurement = encoder->GetPeriod();
		if (measurement < 0.1) {
			measurements[measurementNumber] = measurement;
			measurementNumber++;
			if (measurementNumber > 4) measurementNumber = 0;

		}
		int i;
		double period = 0;
		for(i=0;i<5;i++) period += measurements[i];
		period /= 5.0;
		
		if (period == 0 || period > 0.1)
			return 0;
		double encoderSpeed = (WHEEL_CIRCUMFERENCE/ENCODER_TICKS_PER_REVOLUTION)
				/ period;
		cout << "Encoder speed: " << encoderSpeed << "\n";
		return encoderSpeed;
	}

	// the functions below can be used outside this class
public:
	double maxAcceleration;	
	
	EncoderSpeedController(SpeedController *controller, Encoder *enc)
	{
		speedController = controller;
		encoder = enc;
		jaguar_speed = 0.0;
		timer.Start();
		lastTime = timer.Get();
		
		measurementNumber = 0;
		
		SetCoefficients();
	}
	
	EncoderSpeedController(SpeedController *controller, Encoder *enc, double maxAccel) {
		speedController = controller;
		encoder = enc;
		jaguar_speed = 0.0;
		timer.Start();
		lastTime = timer.Get();
		maxAcceleration = maxAccel;
	}
	
	void SetMaxAccel(double accel){
		maxAcceleration = accel;
	}
	
	void SetCoefficients(double a = 1, double b = 2) {
		this->a = a;
		this->b = b;
	}
	
	
	// set a velocity, ignoring max acceleration

	
	void SetVelocity(float speed) {

		double currentTime = timer.Get();
		double timeDifference = currentTime - lastTime;
		lastTime = currentTime;
		double encoderSpeed = calculateEncoderSpeed();
		if (!(encoder->GetDirection())) encoderSpeed *= -1.0; // switch if going backwards
		//cout << "EncoderSpeed:" << encoderSpeed << "\n";
		double velocityDifference = fabs(encoderSpeed - speed) * 1;
		if (encoderSpeed > speed){ // then we have to slow down
			//jaguar_speed -= (ENCODER_SPEED_INCREMENT + velocityDifference) * timeDifference;
			jaguar_speed -= (a*pow(velocityDifference,b) *timeDifference);
			//cout << jaguar_speed << "Slowing down \n";
		}
		else if (encoderSpeed < speed) { //then we have to speed up
			//jaguar_speed += (ENCODER_SPEED_INCREMENT + velocityDifference) * timeDifference;
			jaguar_speed += (a*pow(velocityDifference,b) *timeDifference);
			//cout << jaguar_speed << "Speeding up \n";
		}
		cout << (a*pow(velocityDifference,b) *timeDifference) << "\n";
		
		if (jaguar_speed > 1)
			jaguar_speed = 1;
		else if (jaguar_speed < -1)
			jaguar_speed = -1;
		
		speedController->Set(jaguar_speed);
		//cout << "Jaguar Speed:" << jaguar_speed << "\n";
		//cout << "Increment:" << ENCODER_SPEED_INCREMENT * timeDifference << "\n";
	}
	
	// set a velocity, taking account of max acceleration
	void Set(float speed) {
		double currentTime = timer.Get();
		double timeDifference = currentTime - lastTime;
		double encoderSpeed = calculateEncoderSpeed();
		if (!(encoder->GetDirection())) encoderSpeed *= -1.0; // switch if going backwards
		
		double intendedAccel = (speed - encoderSpeed)/timeDifference;
		
		if (fabs(intendedAccel) > maxAcceleration) { //the absolute value accounts for both speeding up and slowing down
			// increase velocity by (vi + maxAccel*t)
			speed = encoderSpeed + (maxAcceleration * timeDifference);
		}
		SetVelocity(speed);
	}
	
	
	
	


//	
//	void Set(float speed, bool ) {
//		// TODO!
//		// this function must be called to update the motors
//		// assume speed is in meters per second
//		double currentTime = timer.Get();
//		double timeDifference = currentTime - lastTime;
//		double encoderSpeed = calculateEncoderSpeed();
//		
//		if ((encoderSpeed - speed) > ENCODER_SPEED_DEADBAND) { //if actual speed is greater than intended speed
//			jaguar_speed -= ENCODER_SPEED_INCREMENT * timeDifference * accelRatio; 
//			//cout << encoderSpeed << "decreasing";
//		}
//		else if ((speed - encoderSpeed) > ENCODER_SPEED_DEADBAND) { // if actual speed is less than intended speed
//			jaguar_speed += ENCODER_SPEED_INCREMENT * timeDifference * accelRatio;
//			//cout << encoderSpeed << "increasing";
//		}
//		if (jaguar_speed > 1)
//			jaguar_speed = 1;
//		else if (jaguar_speed < -1)
//			jaguar_speed = -1;
//		if (speed == 0) { //what does this section do?
//			if ((jaguar_speed < 0.2) && (jaguar_speed > 0)) {
//				jaguar_speed = 0.2;
//			} else if ((jaguar_speed > -0.2) && (jaguar_speed < 0)) {
//				jaguar_speed = -0.2;
//			}
//		}
//		//cout << "jaguar speed:" << jaguar_speed << "\n";
//		speedController->Set(jaguar_speed);
//		lastTime = currentTime;
//	}
	float Get(void) {
		// return float for compatablility with SpeedController ?
		return (float)calculateEncoderSpeed();
	}
}; 

#endif
